#include <objLoader.h>
#include <surftrack.h>
#include <subdivisionscheme.h>
#include <vector>
#include <gl/glut.h>
#include "GLUtil.h"
#include "SolverUtil.h"
#include "BEMUtil.h"
#include "EvalUtil.h"
#include <stdio.h>
#include <omp.h>


#pragma comment(lib, "cblas")
#pragma comment(lib, "gsl")

using namespace std;

static GLint    ImageWidth=800;
static GLint    ImageHeight=800;

double shiftAlpha, shearModulus, poissonRatio, Pi;

objLoader *objdata;

SurfTrack* surf = NULL; 

double* u;


void initELTopo()
{
	SurfTrackInitializationParameters params;

	params.m_min_edge_length = 0.01;
	params.m_max_edge_length = 4;
	params.m_max_volume_change = 0.025;
	params.m_min_triangle_angle = 15;
	params.m_max_triangle_angle = 165;
	params.m_min_triangle_area = 0.00001;
	params.m_merge_proximity_epsilon = 0.00001;
	params.m_proximity_epsilon = 0.00001;
	params.m_friction_coefficient = 1;

	params.m_use_curvature_when_splitting = false;
	params.m_use_curvature_when_collapsing = false;
	params.m_perform_improvement = true;
	params.m_allow_topology_changes = true;
	params.m_collision_safety = false;
	params.m_allow_non_manifold = false;
	params.m_allow_vertex_movement = false;

	params.m_subdivision_scheme = std::make_shared<MidpointScheme>();

	params.m_min_curvature_multiplier;
	params.m_max_curvature_multiplier;

	vector<Vec3ui> tris;
	vector<Vec3d> verts;
	vector<double> masses;

	for (int i=0; i<objdata->vertexCount; i++)
	{
		obj_vector* v = objdata->vertexList[i];

		verts.push_back(Vec3d(v->e[0], v->e[1], v->e[2]));
		masses.push_back(1);
	}

	for (int i=0; i<objdata->faceCount;i++)
	{
		obj_face* o = objdata->faceList[i];

		tris.push_back(Vec3ui(o->vertex_index[0], o->vertex_index[1], o->vertex_index[2]));
	}

	surf = new SurfTrack(verts, tris, masses, params);

	surf->defrag_mesh();
	surf->m_collision_pipeline.assert_mesh_is_intersection_free(false);

	surf->improve_mesh();
	surf->topology_changes();
	surf->defrag_mesh();
}

void renderMesh()
{
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	glBegin(GL_TRIANGLES);

	for (int i = 0; i<surf->m_mesh.num_triangles(); i++)
	{
		Vec3ui tri = surf->m_mesh.get_triangle(i);			

		glNormal3d(surf->get_vertex_normal(tri[0])[0], surf->get_vertex_normal(tri[0])[1], surf->get_vertex_normal(tri[0])[2]);
		glVertex3d(surf->get_position(tri[0])[0]+u[tri[0]*3+0], surf->get_position(tri[0])[1]+u[tri[0]*3+1], surf->get_position(tri[0])[2]+u[tri[0]*3+2]);

		glNormal3d(surf->get_vertex_normal(tri[1])[0], surf->get_vertex_normal(tri[1])[1], surf->get_vertex_normal(tri[1])[2]);
		glVertex3d(surf->get_position(tri[1])[0]+u[tri[1]*3+0], surf->get_position(tri[1])[1]+u[tri[1]*3+1], surf->get_position(tri[1])[2]+u[tri[1]*3+2]);		

		glNormal3d(surf->get_vertex_normal(tri[2])[0], surf->get_vertex_normal(tri[2])[1], surf->get_vertex_normal(tri[2])[2]);
		glVertex3d(surf->get_position(tri[2])[0]+u[tri[2]*3+0], surf->get_position(tri[2])[1]+u[tri[2]*3+1], surf->get_position(tri[2])[2]+u[tri[2]*3+2]);
	}

	glEnd();
}


void test()
{
	int n = surf->m_mesh.num_triangles() * 3;
	int m = surf->get_num_vertices() * 3;

	double* b = (double*)malloc(n * sizeof(double));
	double* x = (double*)malloc(n * sizeof(double));
	double* F = (double*)malloc(m * 3 * sizeof(double));
	u = (double*)malloc(m * sizeof(double));

	vectorClear(&x, n);
	assemblyBoundaryVector(surf, &b);

	biCGStab(myMatrixVectorProduct, b, &x, n, 10000, 1e-6);

	evaluateDisplacementField(x, &u);

	/*evaluateDeformationGradientField(x, &F);

	for (int i=0; i<surf->get_num_vertices(); i++)
	{
		for (int a=0; a<3; a++)
		{
			for (int b=0; b<3; b++)
			{
				printf("%f ", F[getTensorArrayIndex(a,b,i)]);
			}
			printf("\n");
		}
		printf("#########################\n");
	}*/
}

int main(int argc, char **argv)
{
	objdata = new objLoader();
	objdata->load("cube3x3.obj");

	Pi = 3.1415926;
	shearModulus = 1;
	poissonRatio = 0;
	shiftAlpha = 1e-3;

	initELTopo();

	free(objdata);

	test();

	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
	glutInitWindowPosition(100, 100);
	glutInitWindowSize(ImageWidth, ImageHeight);
	glutCreateWindow("FLIP");
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutKeyboardFunc(keyboard);

	glutMainLoop();


	return 0;

}



